/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package frc2856.robot;
import edu.wpi.first.wpilibj.Joystick;
/*
 * @author Micheal "Supreme Commander of the 23rd Gay Battallion of the Green Beret Homosex-Rangers" Hallways
 */
public class UserInterface {
    Joystick leftStick, rightStick;
    HolonomicDrive holoDrive;
    Shooter shooter;
    Climber climber;
    Aiming aiming;
    CustomButton[] leftButtons, rightButtons;
    //hackertyper.net
    //aaron has never seen firefly
    
    public UserInterface(){
        this(null, null, null, null);
    }
    
    public UserInterface(HolonomicDrive hd, Shooter sh, Climber c, Aiming ai){
        holoDrive = hd;
        shooter = sh;
        climber = c;
        aiming = ai;
        leftStick = new Joystick(RobotMap.leftJoystickPort);
        rightStick = new Joystick(RobotMap.rightJoystickPort);
        setupButtons();
    }
    
    public void update(){
        periodicButtons();
        
        float driveMag = (float) leftStick.getMagnitude();//(float) Math.sqrt(leftStick.getY()*leftStick.getY()+leftStick.getX()*leftStick.getX());
        float driveDir = (float) leftStick.getDirectionDegrees();
        float driveRot = (float) rightStick.getX();
        if((Math.abs((double)driveMag)<0.025))
            driveMag = 0;
        if((Math.abs((double)driveRot)<0.025))
            driveRot = 0;
        
        holoDrive.drive(driveMag, driveDir, driveRot);
        
	/*if((leftButtons[4].getRawStatus() && leftButtons[4].getChangedStatus(true)) ||
		(leftButtons[5].getChangedStatus(true) && leftButtons[5].getRawStatus())) {
            holoDrive.resetGyro();
	    holoDrive.setUseGyro(!holoDrive.isUsingGyro());
            String s = "Gyro ";
            if(holoDrive.isUsingGyro())
                s+=" IS being used";
            else
                s+=" IS NOT being used";
            System.out.println(s);
	}*/
        
        if(leftButtons[4].getChangedStatus(true)){
            System.out.println("gyro angle: "+holoDrive.gyro.getAngle());
        }
        if(leftButtons[5].getChangedStatus(true)){
            holoDrive.resetGyro();
        }
        /*
        if(leftButtons[6].getChangedStatus(true)){
            climber.toggleCompressor();
        }*/
        if(leftButtons[7].getChangedStatus(true)){
            climber.togglePiston();
        }
        
        if(leftButtons[8].getChangedStatus(true)){
            holoDrive.incrementGyro();
        }
        if(leftButtons[9].getChangedStatus(true)){
            holoDrive.decrementGyro();
        }
        if(leftButtons[10].getChangedStatus(true)){
            holoDrive.resetGyro();
            //System.out.println("MORGAN IS A MORMON");
        }
        if(leftButtons[11].getChangedStatus(true)){
            holoDrive.useGyro = !holoDrive.useGyro;
        }
        
        if(rightButtons[1].getChangedStatus(true)){
            shooter.shoot(true);
        }
        else if(rightButtons[1].getChangedStatus(false)){
            shooter.unshoot();
        }
        
        if(rightButtons[2].getChangedStatus(true)){
            aiming.raise();
        }
        if(rightButtons[3].getChangedStatus(true)){
            aiming.lower();
        }
        if(rightButtons[6].getChangedStatus(true)){
            aiming.autoAim();
        }
        if(rightButtons[8].getChangedStatus(true)){
            shooter.toggleShooter();
            
        }
        if(rightButtons[9].getChangedStatus(true)){
            System.out.println(shooter.getEffort());
        }
        if(shooter.shooterEnabled) {
            shooter.spinEffort((-1*rightStick.getZ()+1)/2);
        }
        else{
            shooter.spinEffort(0);
        }
    }
    
    private void setupButtons() {
        //System.out.println("Setting Up Buttons");
	leftButtons = new CustomButton[12];
	rightButtons = new CustomButton[12];
        for(int i = 1; i<12; i++){
            leftButtons[i] = new CustomButton(leftStick, i);
            rightButtons[i] = new CustomButton(rightStick, i);
        }
    }
    
    private void periodicButtons() {
        for(int i = 1; i<12; i++){
            leftButtons[i].periodicChanged();
            rightButtons[i].periodicChanged();
        }
    }
    
    private class CustomButton{
        Joystick joy;
        int index;
        boolean changed = false;
        boolean status = false;
        public CustomButton(Joystick j, int i){
            joy = j;
            index = i;
            status = joy.getRawButton(index);
        }
        
        public boolean getRawStatus(){
            status = joy.getRawButton(index);
            return joy.getRawButton(index);
        }
        
        public boolean getChangedStatus(boolean compareTo){
            return (changed) && (status == compareTo);
        }
        
        public void periodicChanged(){
            changed = (status != joy.getRawButton(index));
            status = joy.getRawButton(index);
        }
    }
}
